//
// Created by 张祎鹤严 on 2022/12/30.
//

#include "driver_superc.h"
#include "bsp_judge.h"
#include "driver_chassis.h"
#include "auto_superc.h"
enum SuperCapState super_cap_state = READY;

Super_Cap_t superCap = {
        .scSet.enable = 0
};

void SC_recv_message(uint8_t *data)
{
    superCap.power_fb.p_sc = (int16_t)(data[0] | (data[1] << 8)) / 50.0f;//100.f;           //0.01W/LSB


    float _p_wheel = (int16_t)(data[2] | (data[3] << 8)) /50.0f;// 100.f;                   //0.01W/LSB
    //直接过滤掉所有小于0（因为受到场上因素的干扰，所以会导致有些时候功率的小于0，这部分直接过滤掉即可）
    _p_wheel = _p_wheel < 0 ? 0 : _p_wheel;
    //低通过滤掉尖峰点，减少噪声（系数决定了过滤程度的大小——这当然是由发送数据的准确与否相关）
    superCap.power_fb.p_wheel = superCap.power_fb.p_wheel * 0.6f + _p_wheel * 0.4f;

    //mV -> V
    float V_SC = (int16_t)(data[4] | (data[5] << 8)) / 1000.f;
    //低通过滤掉尖峰点，减少噪声
    superCap.scFeedback.V_SC = superCap.scFeedback.V_SC * 0.9f + V_SC * 0.1f;

    //电容的状态是什么
    super_cap_state = data[6];

    if (super_cap_state >= FULL && super_cap_state <= EMPTY)
        LostCounterFeed(SUPERC_LOST_COUNT);

}

void SC_send_message(Super_Cap_t *super_cap)
{
    uint8_t send_buf[8];

    // 电容总开关 1开 0关
    send_buf[0] = super_cap->scSet.enable;
    // 限幅
    super_cap->scSet.set_power_charge = super_cap->scSet.set_power_charge > POWER_CHARGE_MAX ? POWER_CHARGE_MAX : super_cap->scSet.set_power_charge;

    super_cap->scSet.set_power_charge = super_cap->scSet.set_power_charge < -POWER_DISCHARGE_MAX ? -POWER_DISCHARGE_MAX : super_cap->scSet.set_power_charge;
    // 电容功率单位换算 W -> mW -> 0.1mW
    int32_t power_charge = (int32_t)(super_cap->scSet.set_power_charge * 1000 * 10);   // 0.1 mW/LSB

    send_buf[1] = (uint8_t)(power_charge & 0xff);
    send_buf[2] = (uint8_t)((power_charge >> 8) & 0xff);
    send_buf[3] = (uint8_t)((power_charge >> 16) & 0xff);
    send_buf[4] = (uint8_t)((power_charge >> 24) & 0xff);

    CAN1_Send_Msg(send_buf,8,0x312);
}

extern uint32_t LostCounterCountNumber[NUMBERS_OF_COUNT];

PID super_cap_pid;
float Power_delta = 30.0f;
float super_cap_ref = 55.0f;
float power_charge_test = 0;
void SC_PID_Init(void)
{
    super_cap_pid.Kp = SC_KP;
    super_cap_pid.Ki = SC_KI;
    super_cap_pid.Kd = SC_KD;
    super_cap_pid.OutMax =  1;
    super_cap_pid.calc = &PidCalc;
    super_cap_pid.clear = &PidClear;

    super_cap_pid.clear(&super_cap_pid);
}
float last_power_charge = 0;
float Power_Charge_Max = 0.f;
float SC_power_cacl()
{
    /**
     * 电容功率环
     * 缓冲能量PID + 前馈调节
     *
     *                  抽象电容板
     *      -———————————————————————————————-
     *      |                               |
     *  <————————<————-                     |
     * V_SC |         | I_IN_SC             |
     *      |         ↑                     |
     *  ——>——————>————|                     |
     * V_IN |         ↓ I_CHASSIS           |
     *      |         |                     |
     *      -—————————|—————————————————————-
     *                ↓ Motors
     */
    /******  获取裁判系统数据  ******/
    float chassis_power_judge_limit = judge_rece_mesg.game_robot_state_data.chassis_power_limit;
    float chassis_power = (float)judge_rece_mesg.power_heat_data.chassis_power;
    float chassis_power_buffer = judge_rece_mesg.power_heat_data.chassis_power_buffer;
    //这个系数是使底盘缓冲能量大于多少的时候进行充电
    super_cap_ref = 45.0f;
    /****** PID + 前馈  ******/
    if (super_cap_state != READY)
        super_cap_pid.clear(&super_cap_pid);//置0
    super_cap_pid.Ref = super_cap_ref;
    //feedback值为缓冲能量值
    super_cap_pid.Fdb = chassis_power_buffer;
    super_cap_pid.calc(&super_cap_pid);
    float Power_pid = -super_cap_pid.Out * Power_delta;
    //算出来的前馈值
    float Power_f = chassis_power_judge_limit - superCap.power_fb.p_wheel + 15.0f;
    float Power_charge = Power_f + Power_pid;
    Power_Charge_Max = chassis_power + 20.0f;
    // 限幅
    // 电容放电功率不能超过底盘轮耗电功率 否则多放出的电会全部木大,反向放给电源管理,白白浪费
    if(Power_charge > 0){
        //超级电容开启的时候，必须将充电限制到一个比较小的值，否则先进行了充电，之后立马又放电，会导致出现很大的电流值
        if(chassisControlData.disCharging == WORK) {
            if (Power_charge > POWER_CHARGE_MAX_SC)
                Power_charge = POWER_CHARGE_MAX_SC;
        }
        else
            if (Power_charge > Power_Charge_Max)
                Power_charge = Power_Charge_Max;
    }

    else if(Power_charge < -superCap.power_fb.p_wheel)
        Power_charge = -superCap.power_fb.p_wheel;
    last_power_charge = Power_charge;
    power_charge_test = Power_charge;
    return Power_charge;
}

uint8_t SC_DEBUG_FLAG = 0;      //调试用flag
float SC_CHARGE_TEST = 0;       //调试用 电容功率设定值
void super_cap_control(void)
{
    /*
     * 新电容控制程序
     * SuperFlag仅代表操作手对电容放电与否的控制
     * 操作手不需要也做不到对电容充电与否进行控制 这部分控制（包括防止抢电）应该是电控的任务
     *
     * 底盘功率环需要3个输入：缓冲能量 功率上限 实时功率反馈
     * 使用电容放电:      缓冲能量         固定为60
     *                  底盘功率上限      固定为120w   由电容功率环来保证不会超功率
     *                  实时功率反馈      由电容板测量提供（SC_get_power_wheel()）
     * 不使用电容放电:     缓冲能量         从裁判系统获取
     *                  底盘功率上限      从裁判系统获取
     *                  实时功率反馈      依旧由电容板测量提供（SC_get_power_wheel()）  如果从裁判系统获取会有抢电问题
     */
    float set_power_charge = SC_power_cacl();

    /******  电容状态切换控制  ******/
    switch (super_cap_state) {
        // 电容电放完了 不能放电
        case EMPTY: {
            set_power_charge = set_power_charge < 0 ? 0 : set_power_charge;
            if(!superCap.scSet.enable && superCap.scSet.set_power_charge > 0)
                superCap.scSet.enable = 1;
            break;
        }
        // 电容就绪 可充可放
        case READY: {
            if(!superCap.scSet.enable && superCap.scSet.set_power_charge > 0)
                superCap.scSet.enable = 1;
            break;
        }
        // 电容电满 不需要继续充电
        case FULL: {
            set_power_charge = set_power_charge > 0 ? 0 : set_power_charge;
            if(superCap.scSet.enable)
                superCap.scSet.enable = 0;
            break;
        }
        default: {
            set_power_charge = 0;
            break;
        }
    }
    if ((GetErrorState() >> SUPERC_LOST_COUNT) & 0x0001) {
        superCap.scSet.enable = 0;
        chassisControlData.disCharging = 0;                                 // 出问题关电容
    }
    else if (chassisControlData.superFlag && (super_cap_state == READY || super_cap_state == FULL)) {
            if (!superCap.scSet.enable)
                superCap.scSet.enable = 1;
            chassisControlData.disCharging = 1;                                 // 只要电容有电就放
    }
    else if(chassisControlData.disCharging_Auto && (super_cap_state == READY || super_cap_state == FULL))
    {
        if (!superCap.scSet.enable)
            superCap.scSet.enable = 1;
    }
    else {
        chassisControlData.disCharging = 0;
        //todo:
        gimbalControlData.superFlag = 0;
        set_power_charge = set_power_charge < 0 ? 0 : set_power_charge;     // 禁止电容放电
    }
    if (SC_DEBUG_FLAG)
        superCap.scSet.set_power_charge = SC_CHARGE_TEST;
    else
        superCap.scSet.set_power_charge = set_power_charge;

    //下面的这俩if都是起保护的作用
    if(IF_Dead()){
        superCap.scSet.enable = 0;
        superCap.scSet.set_power_charge = 0;
    }
    if(IF_VOL()){
        superCap.scSet.enable = 0;
        superCap.scSet.set_power_charge = 0;
    }

    SC_send_message(&superCap);
    SC_Auto_Enable();
}

float SC_get_power_wheel()
{
    return superCap.power_fb.p_wheel;
}

//血量判定
int IF_Dead(){
    if(judge_rece_mesg.game_robot_state_data.remain_HP <= 0)
        return 1;
    else
        return 0;
}

//电压判定
int IF_VOL(){
    if(judge_rece_mesg.power_heat_data.chassis_volt <= 0)
        return 1;
    else
        return 0;
}